#include "cloud_transform.hpp"
#include <iostream>

void printUsage(const char* programName) {
    std::cout << "Usage: " << programName 
              << " <old_cloud.ply> <new_cloud.ply> <pose_info.txt>" << std::endl;
}

int main(int argc, char** argv) {
    if (argc != 4) {
        printUsage(argv[0]);
        return 1;
    }
    
    std::string old_cloud_path = argv[1];
    std::string new_cloud_path = argv[2];
    std::string pose_file_path = argv[3];
    
    CloudTransform transformer;
    
    // Load point clouds
    if (!transformer.loadPointClouds(old_cloud_path, new_cloud_path)) {
        std::cerr << "Failed to load point clouds" << std::endl;
        return 1;
    }
    
    // Load pose information
    if (!transformer.loadPoseInfo(pose_file_path)) {
        std::cerr << "Failed to load pose information" << std::endl;
        return 1;
    }
    
    // Transform the new cloud to the old cloud's frame
    transformer.transformNewCloud();
    
    // Evaluate the difference between clouds
    double difference = transformer.evaluateCloudDifference();
    
    if (difference >= 0) {
        std::cout << "Average point-to-point distance: " << difference << " meters" << std::endl;
    }
    
    return 0;
}
